﻿//
// This file is part of AVSReflector project
// Copyright (C) 2011 Yuan Pengfei
//

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;


namespace AVSReflector.Core
{
    public struct MotionVector
    {
        public int x, y;
        public MotionVector(int x, int y)
        {
            this.x = x;
            this.y = y;
        }
        public static MotionVector zero()
        {
            MotionVector ret;
            ret.x = 0;
            ret.y = 0;
            return ret;
        }
        public static bool operator ==(MotionVector mv1, MotionVector mv2)
        {
            return (mv1.x == mv2.x && mv1.y == mv2.y);
        }
        public static bool operator !=(MotionVector mv1, MotionVector mv2)
        {
            return (mv1.x != mv2.x || mv1.y != mv2.y);
        }
        public static bool operator ==(MotionVector mv, int val)
        {
            return (mv.x == val && mv.y == val);
        }
        public static bool operator !=(MotionVector mv, int val)
        {
            return (mv.x != val || mv.y != val);
        }
        public override int GetHashCode()
        {
            return (x << 16) + y;
        }
        public override bool Equals(object o)
        {
            if (o.GetType() != typeof(MotionVector))
                return false;
            return (this == (MotionVector)o);
        }
    }

    public enum BlockPredMode
    {
        Skip,
        Direct,
        Symmetric,
        Forward,
        Backward
    }

    public class Block : INode
    {
        public bool all_zeros;
        public int id;
        public int distance_index;
        public Picture picture;
        public int pos_x;
        public int pos_y;

        public int[] level;
        public int[] run;
        public int[,] quant_coeff_matrix;
        public int[,] coeff_matrix;
        public int[,] residue_matrix;

        public int[,] pred_matrix;
        public int[,] pred_matrix_fw;
        public int[,] pred_matrix_bw;
        public int[,] rec_matrix;

        public int intra_pred_mode;
        bool intra;

        public BlockPredMode inter_pred_mode;
        public int block_distance;
        public int block_distance_fw;
        public int block_distance_bw;
        public MotionVector mvDiff;
        public MotionVector mvE;
        public MotionVector mvFw;
        public MotionVector mvBw;
        public Block ref_block;
        public Block ref_block_fw;
        public Block ref_block_bw;
        public int ref_index;
        public int ref_index_fw;
        public int ref_index_bw;

        static string ArrayToString(int[] array)
        {
            if (array == null)
                return null;
            StringBuilder sb = new StringBuilder();
            for (int i = 0; i < array.Length - 1; ++i)
                sb.AppendFormat("{0,3} ", array[i]);
            sb.AppendFormat("{0,3} ", array.Last());
            return sb.ToString();
        }

        static string[] MatrixToLines(int[,] matrix, bool hex=false)
        {
            if (matrix == null)
                return null;
            List<string> lines = new List<string>();
            for (int j = 0; j < matrix.GetLength(1); ++j)
            {
                StringBuilder sb = new StringBuilder();
                if (hex)
                {
                    for (int i = 0; i < matrix.GetLength(0) - 1; ++i)
                        sb.AppendFormat("{0,2:X} ", (byte)matrix[i, j]);
                    sb.AppendFormat("{0,2:X} ", (byte)matrix[matrix.GetLength(0) - 1, j]);
                }
                else
                {
                    for (int i = 0; i < matrix.GetLength(0) - 1; ++i)
                        sb.AppendFormat("{0,5} ", matrix[i, j]);
                    sb.AppendFormat("{0,5} ", matrix[matrix.GetLength(0) - 1, j]);
                }
                lines.Add(sb.ToString());
            }
            return lines.ToArray();
        }

        public string[] GetInfo()
        {
            List<string> lines = new List<string>();
            if (id < 4)
            {
                lines.Add("\t--- Luma Block " + id + " ---");
                if (intra)
                {
                    switch (intra_pred_mode)
                    {
                        case 0:
                            lines.Add("Intra_8x8_Vertical");
                            break;
                        case 1:
                            lines.Add("Intra_8x8_Horizontal");
                            break;
                        case 2:
                            lines.Add("Intra_8x8_DC");
                            break;
                        case 3:
                            lines.Add("Intra_8x8_Down_Left");
                            break;
                        case 4:
                            lines.Add("Intra_8x8_Down_Right");
                            break;
                    }
                }
                else
                {
                    lines.Add("Inter luma pred mode: " + NameOfBlockPredMode(inter_pred_mode));
                    if (id < 4)
                    {
                        lines.Add(string.Format("MV diff = ({0}, {1})", mvDiff.x, mvDiff.y));
                        if (inter_pred_mode == BlockPredMode.Backward || inter_pred_mode == BlockPredMode.Forward
                            || ref_block_fw == null || ref_block_bw == null)
                        {
                            lines.Add(string.Format("Motion vector = ({0}, {1})", mvE.x, mvE.y));
                            lines.Add("Reference index = " + ref_index);
                            lines.Add("Block distance = " + block_distance);
                        }
                        else
                        {
                            lines.Add(string.Format("Motion vector: forward = ({0}, {1}) ; backward = ({2}, {3})",
                                mvFw.x, mvFw.y, mvBw.x, mvBw.y));
                            lines.Add(string.Format("Reference index: forward = {0} ; backward = {1}",
                                ref_index_fw, ref_index_bw));
                            lines.Add(string.Format("Block distance: forward = {0} ; backward = {1}",
                                block_distance_fw, block_distance_bw));
                        }
                    }
                }
            }
            else
            {
                lines.Add("\t--- Chroma Block " + (id - 4) + " ---");
                if (intra)
                {
                    switch (intra_pred_mode)
                    {
                        case 0:
                            lines.Add("Intra_Chroma_DC");
                            break;
                        case 1:
                            lines.Add("Intra_Chroma_Horizontal");
                            break;
                        case 2:
                            lines.Add("Intra_Chroma_Vertical");
                            break;
                        case 3:
                            lines.Add("Intra_Chroma_Plane");
                            break;
                    }
                }
            }
            if (all_zeros)
            {
                lines.Add("\tlevel = 0");
                lines.Add("\trun = 0");
                lines.Add("\tquant_coeff_matrix = 0");
                lines.Add("\tcoeff_matrix = 0");
                lines.Add("\tresidue_matrix = 0");
            }
            else
            {
                lines.Add("\tlevel[" + level.Length + "] =");
                lines.Add(ArrayToString(level));
                lines.Add("\trun[" + run.Length + "] =");
                lines.Add(ArrayToString(run));
                lines.Add(string.Format("\tquant_coeff_matrix[{0}, {1}] =",
                    quant_coeff_matrix.GetLength(1), quant_coeff_matrix.GetLength(0)));
                lines.AddRange(MatrixToLines(quant_coeff_matrix));
                lines.Add(string.Format("\tcoeff_matrix[{0}, {1}] =",
                    coeff_matrix.GetLength(1), coeff_matrix.GetLength(0)));
                lines.AddRange(MatrixToLines(coeff_matrix));
                lines.Add(string.Format("\tresidue_matrix[{0}, {1}] =",
                    residue_matrix.GetLength(1), residue_matrix.GetLength(0)));
                lines.AddRange(MatrixToLines(residue_matrix));
            }
            lines.Add(string.Format("\tpred_matrix[{0}, {1}] =",
                pred_matrix.GetLength(1), pred_matrix.GetLength(0)));
            lines.AddRange(MatrixToLines(pred_matrix, true));
            lines.Add(string.Format("\trec_matrix[{0}, {1}] =",
                rec_matrix.GetLength(1), rec_matrix.GetLength(0)));
            lines.AddRange(MatrixToLines(rec_matrix, true));
            lines.Add("");
            return lines.ToArray();
        }

        public static Block Parse(BitStream s, int id)
        {
            Block ret = new Block();
            ret.id = id;
            ret.ref_index = Parameters.ReferenceIndex;
            ret.distance_index = Parameters.DistanceIndex;
            ret.picture = Context.currentPicture;
            ret.intra = Context.currentMB.intra;
            if (ret.intra)
                ret.ref_index = -1;
            ret.inter_pred_mode = Parameters.InterBlockPredMode;
            ret.pos_x = (int)((Context.currentMB.mb_index % Context.currentSequence.mb_width) * 16 + (id % 2) * 8);
            ret.pos_y = (int)((Context.currentMB.mb_index / Context.currentSequence.mb_width) * 16 + (id / 2) * 8);

            if (!ret.intra && ret.id < 4)
            {
                ret.mvDiff = new MotionVector(Parameters.MVDiffX, Parameters.MVDiffY);
                ret.GetReferenceBlocksAndMotionVectors();
                ret.GetBlockDistances();
                ret.PredictMotionVectors();
                ret.GetReferenceBlocksAndMotionVectors();   // TODO: this is bad
            }

            if (!(id < 6 && (Parameters.MbCBP & (1 << id)) != 0
                || id >= 6 && (Parameters.MbCBP422 & (1 << (id - 6))) != 0))
                ret.all_zeros = true;
            else
            {
                ret.all_zeros = false;
                bool intra = Context.currentMB.intra;
                bool luma = id < 4;
                uint trans_coefficient = 0;
                int escape_level_diff = 0;
                int absLevel, maxAbsLevel = 0;
                List<int> Level = new List<int>(), Run = new List<int>();
                VLCTable v;
                if (luma)
                {
                    if (intra)
                        v = new VLCTable(VLCTableType.Intra);
                    else
                        v = new VLCTable(VLCTableType.Inter);
                }
                else
                    v = new VLCTable(VLCTableType.Chroma);
                #region Parse Level & Run
                do
                {
                    int k = v.K();
                    trans_coefficient = Decoder.ExpGolomb(s, k);
                    if (trans_coefficient >= 59)
                    {
                        if (intra && luma)
                            escape_level_diff = (int)Decoder.ExpGolomb(s, 1);
                        else
                            escape_level_diff = (int)Decoder.ExpGolomb(s, 0);
                        Run.Add(((int)trans_coefficient - 59) / 2);
                        int reflevel;
                        if (((int)trans_coefficient - 59) / 2 > v.MaxRun())
                            reflevel = 1;
                        else
                            reflevel = v.RefAbsLevel((trans_coefficient - 59) / 2);
                        if (trans_coefficient % 2 == 1)
                            Level.Add(-(reflevel + escape_level_diff));
                        else
                            Level.Add(reflevel + escape_level_diff);
                    }
                    else if (trans_coefficient == v.EOB())
                        break;
                    else
                    {
                        int l = v.Level(trans_coefficient);
                        if (l > 0)
                        {
                            Level.Add(l);
                            Run.Add(v.Run(trans_coefficient));
                        }
                        else
                        {
                            l = v.Level(trans_coefficient - 1);
                            if (l <= 0)
                                throw new Exception("Invalid AVS format!");
                            Level.Add(-l);
                            Run.Add(v.Run(trans_coefficient - 1));
                        }
                    }

                    #region Update current VLC table
                    absLevel = Math.Abs(Level.Last());
                    if (absLevel > maxAbsLevel)
                    {
                        if (luma)
                        {
                            if (intra)
                            {
                                if (absLevel == 1)
                                    v.SetID(1);
                                if (absLevel == 2)
                                    v.SetID(2);
                                if (absLevel == 3 || absLevel == 4)
                                    v.SetID(3);
                                if (absLevel >= 5 && absLevel <= 7)
                                    v.SetID(4);
                                if (absLevel >= 8 && absLevel <= 10)
                                    v.SetID(5);
                                if (absLevel > 10)
                                    v.SetID(6);
                            }
                            else
                            {
                                if (absLevel == 1)
                                    v.SetID(1);
                                if (absLevel == 2)
                                    v.SetID(2);
                                if (absLevel == 3)
                                    v.SetID(3);
                                if (absLevel >= 4 && absLevel <= 6)
                                    v.SetID(4);
                                if (absLevel >= 7 && absLevel <= 9)
                                    v.SetID(5);
                                if (absLevel > 9)
                                    v.SetID(6);
                            }
                        }
                        else
                        {
                            if (absLevel == 1)
                                v.SetID(1);
                            if (absLevel == 2)
                                v.SetID(2);
                            if (absLevel == 3 || absLevel == 4)
                                v.SetID(3);
                            if (absLevel > 4)
                                v.SetID(4);
                        }
                        maxAbsLevel = absLevel;
                    }
                    #endregion
                } while (true);
                ret.level = Level.ToArray();
                ret.run = Run.ToArray();
                #endregion

                // Reverse scan
                int[] q = new int[64];
                int pass = Level.Count - 1, num = -1;
                while (pass >= 0)
                {
                    num += Run[pass] + 1;
                    q[num] = Level[pass];
                    pass--;
                }
                ret.quant_coeff_matrix = new int[8, 8];
                Decoder.Rscan(q, ret.quant_coeff_matrix);

                // Anti-quantization
                int qp = Parameters.CurrentQP;
                if (id >= 4)
                    qp = Decoder.ChromaQP(qp);
                ret.coeff_matrix = new int[8, 8];
                for (int i = 0; i < 8; ++i)
                    for (int j = 0; j < 8; ++j)
                        ret.coeff_matrix[i, j] = (ret.quant_coeff_matrix[i, j] * Decoder.DequantTable(qp)
                            + (1 << (Decoder.ShiftTable(qp) - 1))) >> Decoder.ShiftTable(qp);

                // Anti-transformation
                ret.residue_matrix = new int[8, 8];
                Decoder.Atransform(ret.coeff_matrix, ret.residue_matrix);
            }

            ret.pred_matrix = new int[8, 8];
            #region Intra prediction
            if (ret.intra)
            {
                int[,] I = null;
                int[] r = new int[18], c = new int[18];
                int x0 = (int)(Parameters.MbIndex % Context.currentSequence.mb_width) * 16,
                    y0 = (int)(Parameters.MbIndex / Context.currentSequence.mb_width) * 16;
                int lx = x0 + 16, ly = y0 + 16
                    , slicey = (int)Context.currentSlice.slice_vertical_position * 16;
                int dx = 1, dy = 1;
                if (id >= 4)
                {
                    //x0 /= 2;
                    //lx /= 2;
                    dx = 2;
                    if (Context.currentSequence.chroma_format == 1)
                    {
                        //y0 /= 2;
                        //ly /= 2;
                        //slicey /= 2;
                        dy = 2;
                    }
                }
                switch (id)
                {
                    case 0:
                        I = Context.currentPicture.YMatrix;
                        break;
                    case 1:
                        I = Context.currentPicture.YMatrix;
                        x0 += 8;
                        lx = Math.Min(lx + 8, (int)Context.currentSequence.mb_width * 16);
                        ly -= 8;
                        break;
                    case 2:
                        I = Context.currentPicture.YMatrix;
                        y0 += 8;
                        break;
                    case 3:
                        I = Context.currentPicture.YMatrix;
                        x0 += 8;
                        y0 += 8;
                        break;
                    case 4:
                        I = Context.currentPicture.CbMatrix;
                        break;
                    case 5:
                        I = Context.currentPicture.CrMatrix;
                        break;
                    case 6:
                        I = Context.currentPicture.CbMatrix;
                        y0 += 8;
                        break;
                    case 7:
                        I = Context.currentPicture.CrMatrix;
                        y0 += 8;
                        break;
                }
                int ru = 0, cu = 0;
                if (y0 - dy < slicey)
                {
                    for (int i = 1; i <= 16; ++i)
                        r[i] = -1;
                }
                else
                {
                    for (int i = 1; i <= 8; ++i)
                    {
                        if (x0 + i * dx - dx < lx)
                        {
                            r[i] = I[y0 - dy, x0 + i * dx - dx];
                            ru = i;
                        }
                        else
                            r[i] = -1;
                    }
                    for (int i = 9; i <= 16; ++i)
                    {
                        if (x0 + i * dx - dx < lx)
                        {
                            r[i] = I[y0 - dy, x0 + i * dx - dx];
                            ru = i;
                        }
                        else
                            r[i] = r[8];
                        if (r[8] >= 0)
                            ru = i;
                    }
                }
                if (x0 - dx < 0)
                {
                    for (int i = 1; i <= 16; ++i)
                        c[i] = -1;
                }
                else
                {
                    for (int i = 1; i <= 8; ++i)
                    {
                        if (y0 + i * dy - dy < ly)
                        {
                            c[i] = I[y0 + i * dy - dy, x0 - dx];
                            cu = i;
                        }
                        else
                            c[i] = -1;
                    }
                    for (int i = 9; i <= 16; ++i)
                    {
                        if (y0 + i * dy - dy < ly)
                        {
                            c[i] = I[y0 + i * dy - dy, x0 - dx];
                            cu = i;
                        }
                        else
                            c[i] = c[8];
                        if (c[8] >= 0)
                            cu = i;
                    }
                }
                if (x0 >= dx && y0 >= dy)
                    r[0] = I[y0 - dy, x0 - dx];
                else
                {
                    if (r[1] >= 0)
                        r[0] = r[1];
                    else
                        r[0] = c[1];
                }
                c[0] = r[0];
                r[17] = r[16];
                c[17] = c[16];

                for (int i = 0; i < 8; ++i)
                    for (int j = 0; j < 8; ++j)
                        ret.pred_matrix[i, j] = 128;
                if (id < 4)
                {
                    ret.intra_pred_mode = Parameters.IntraLumaPredMode;
                    if (ret.intra_pred_mode == 0 && ru >= 8)
                    {
                        for (int i = 0; i < 8; ++i)
                            for (int j = 0; j < 8; ++j)
                                ret.pred_matrix[i, j] = r[i + 1];
                    }
                    if (ret.intra_pred_mode == 1 && cu >= 8)
                    {
                        for (int i = 0; i < 8; ++i)
                            for (int j = 0; j < 8; ++j)
                                ret.pred_matrix[i, j] = c[j + 1];
                    }
                    if (ret.intra_pred_mode == 2)
                    {
                        if (ru >= 9 && cu >= 9)
                        {
                            for (int i = 0; i < 8; ++i)
                                for (int j = 0; j < 8; ++j)
                                    ret.pred_matrix[i, j] = (((r[i] + 2 * r[i + 1] + r[i + 2] + 2) >> 2)
                                        + ((c[j] + 2 * c[j + 1] + c[j + 2] + 2) >> 2)) >> 1;
                        }
                        else if (ru >= 9)
                        {
                            for (int i = 0; i < 8; ++i)
                                for (int j = 0; j < 8; ++j)
                                    ret.pred_matrix[i, j] = (r[i] + 2 * r[i + 1] + r[i + 2] + 2) >> 2;
                        }
                        else if (cu >= 9)
                        {
                            for (int i = 0; i < 8; ++i)
                                for (int j = 0; j < 8; ++j)
                                    ret.pred_matrix[i, j] = (c[j] + 2 * c[j + 1] + c[j + 2] + 2) >> 2;
                        }
                    }
                    if (ret.intra_pred_mode == 3 && ru >= 16 && cu >= 16)
                    {
                        for (int i = 0; i < 8; ++i)
                            for (int j = 0; j < 8; ++j)
                                ret.pred_matrix[i, j] = (((r[i + j + 1] + 2 * r[i + j + 2] + r[i + j + 3] + 2) >> 2)
                                    + ((c[i + j + 1] + 2 * c[i + j + 2] + c[i + j + 3] + 2) >> 2)) >> 1;
                    }
                    if (ret.intra_pred_mode == 4 && ru >= 16 && cu >= 16)
                    {
                        for (int i = 0; i < 8; ++i)
                            for (int j = 0; j < 8; ++j)
                            {
                                if (i == j)
                                    ret.pred_matrix[i, j] = (c[1] + 2 * r[0] + r[1] + 2) >> 2;
                                else if (i > j)
                                    ret.pred_matrix[i, j] = (r[i - j + 1] + 2 * r[i - j] + r[i - j - 1] + 2) >> 2;
                                else
                                    ret.pred_matrix[i, j] = (c[j - i + 1] + 2 * c[j - i] + c[j - i - 1] + 2) >> 2;
                            }
                    }
                }
                else
                {
                    ret.intra_pred_mode = Parameters.IntraChromaPredMode;
                    if (ret.intra_pred_mode == 0)
                    {
                        if (ru >= 9 && cu >= 9)
                        {
                            for (int i = 0; i < 8; ++i)
                                for (int j = 0; j < 8; ++j)
                                    ret.pred_matrix[i, j] = (((r[i] + 2 * r[i + 1] + r[i + 2] + 2) >> 2)
                                        + ((c[j] + 2 * c[j + 1] + c[j + 2] + 2) >> 2)) >> 1;
                        }
                        else if (ru >= 9)
                        {
                            for (int i = 0; i < 8; ++i)
                                for (int j = 0; j < 8; ++j)
                                    ret.pred_matrix[i, j] = (r[i] + 2 * r[i + 1] + r[i + 2] + 2) >> 2;
                        }
                        else if (cu >= 9)
                        {
                            for (int i = 0; i < 8; ++i)
                                for (int j = 0; j < 8; ++j)
                                    ret.pred_matrix[i, j] = (c[j] + 2 * c[j + 1] + c[j + 2] + 2) >> 2;
                        }
                    }
                    if (ret.intra_pred_mode == 1 && cu >= 8)
                    {
                        for (int i = 0; i < 8; ++i)
                            for (int j = 0; j < 8; ++j)
                                ret.pred_matrix[i, j] = c[j + 1];
                    }
                    if (ret.intra_pred_mode == 2 && ru >= 8)
                    {
                        for (int i = 0; i < 8; ++i)
                            for (int j = 0; j < 8; ++j)
                                ret.pred_matrix[i, j] = r[i + 1];
                    }
                    if (ret.intra_pred_mode == 3 && ru >= 8 && cu >= 8)
                    {
                        int ia = (r[8] + c[8]) << 4, ib = 0, ic = 0;
                        for (int i = 0; i < 4; ++i)
                        {
                            ib += (i + 1) * (r[5 + i] - r[3 - i]);
                            ic += (i + 1) * (c[5 + i] - c[3 - i]);
                        }
                        ib = (17 * ib + 16) >> 5;
                        ic = (17 * ic + 16) >> 5;
                        for (int i = 0; i < 8; ++i)
                            for (int j = 0; j < 8; ++j)
                                ret.pred_matrix[i, j] = Decoder.Clip1((ia + (i - 3) * ib + (j - 3) * ic + 16) >> 5);
                    }
                }
            }
            #endregion
            #region Inter luma prediction
            if (!ret.intra && id < 4)
            {
                if(ret.inter_pred_mode == BlockPredMode.Backward
                    || ret.inter_pred_mode == BlockPredMode.Forward
                    || ret.inter_pred_mode == BlockPredMode.Skip
                    && Context.currentPicture.picture_type != 2)
                {
                    for (int i = 0; i < 8; ++i)
                        for (int j = 0; j < 8; ++j)
                            ret.pred_matrix[i, j] = ret.InterPredictLuma(ret.ref_block.pos_x, 
                                ret.ref_block.pos_y, i, j, ret.mvE, ret.ref_block.picture.YMatrix);
                }
                else
                {
                    ret.pred_matrix_fw = ret.pred_matrix;
                    ret.pred_matrix_bw = new int[8, 8];
                    for (int i = 0; i < 8; ++i)
                        for (int j = 0; j < 8; ++j)
                        {
                            ret.pred_matrix_fw[i, j] = ret.InterPredictLuma(ret.ref_block_fw.pos_x,
                                ret.ref_block_fw.pos_y, i, j, ret.mvFw, ret.ref_block_fw.picture.YMatrix);
                            ret.pred_matrix_bw[i, j] = ret.InterPredictLuma(ret.ref_block_bw.pos_x,
                                ret.ref_block_bw.pos_y, i, j, ret.mvBw, ret.ref_block_bw.picture.YMatrix);
                        }
                }
            }
            #endregion
            #region Inter chroma prediction
            if (!ret.intra && id >= 4)
            {
                Block r;
                int fmt = (int)Context.currentSequence.chroma_format;
                for (int i = 0; i < 8; ++i)
                    for (int j = 0; j < 8; ++j)
                    {
                        if (fmt == 1)
                            r = Context.currentMB.blocks[2 * (j / 4) + i / 4];
                        else
                            r = Context.currentMB.blocks[2 * ((id - 4) / 2) + i / 4];
                        ret.pred_matrix[i, j] = ret.InterPredictChroma(r.ref_block, i, j, r.mvE);
                    }
            }
            #endregion

            #region Reconstruct
            int[,] residue = ret.residue_matrix;
            if (residue == null)
                residue = new int[8, 8];
            ret.rec_matrix = new int[8, 8];
            if (MacroBlock.NameOfType(Context.currentMB.type).Contains("B_"))
            {
                if (ret.inter_pred_mode == BlockPredMode.Backward || ret.inter_pred_mode == BlockPredMode.Forward
                    || id >= 4)
                {
                    for (int i = 0; i < 8; ++i)
                        for (int j = 0; j < 8; ++j)
                            ret.rec_matrix[i, j] = Decoder.Clip1(residue[i, j] + ret.pred_matrix[i, j]);
                }
                else
                {
                    for (int i = 0; i < 8; ++i)
                        for (int j = 0; j < 8; ++j)
                            ret.rec_matrix[i, j] = Decoder.Clip1(residue[i, j]
                                + ((ret.pred_matrix_fw[i, j] + ret.pred_matrix_bw[i, j] + 1) >> 1));
                }
            }
            else
            {
                for (int i = 0; i < 8; ++i)
                    for (int j = 0; j < 8; ++j)
                        ret.rec_matrix[i, j] = Decoder.Clip1(residue[i, j] + ret.pred_matrix[i, j]);
            }
            {
                int[,] I = null;
                int x0 = (int)(Parameters.MbIndex % Context.currentSequence.mb_width) * 16,
                    y0 = (int)(Parameters.MbIndex / Context.currentSequence.mb_width) * 16;
                int dx = 1, dy = 1;
                if (id >= 4)
                {
                    dx = 2;
                    if (Context.currentSequence.chroma_format == 1)
                        dy = 2;
                }
                switch (id)
                {
                    case 0:
                        I = Context.currentPicture.YMatrix;
                        break;
                    case 1:
                        I = Context.currentPicture.YMatrix;
                        x0 += 8;
                        break;
                    case 2:
                        I = Context.currentPicture.YMatrix;
                        y0 += 8;
                        break;
                    case 3:
                        I = Context.currentPicture.YMatrix;
                        x0 += 8;
                        y0 += 8;
                        break;
                    case 4:
                        I = Context.currentPicture.CbMatrix;
                        break;
                    case 5:
                        I = Context.currentPicture.CrMatrix;
                        break;
                    case 6:
                        I = Context.currentPicture.CbMatrix;
                        y0 += 8;
                        break;
                    case 7:
                        I = Context.currentPicture.CrMatrix;
                        y0 += 8;
                        break;
                }
                for (int i = 0; i < 8; ++i)
                    for (int j = 0; j < 8; ++j)
                    {
                        I[y0 + i * dy, x0 + j * dx] = ret.rec_matrix[j, i];
                        if(dx > 1)
                            I[y0 + i * dy, x0 + j * dx + 1] = ret.rec_matrix[j, i];
                        if (dy > 1)
                            I[y0 + i * dy + 1, x0 + j * dx] = ret.rec_matrix[j, i];
                        if (dx > 1 && dy > 1)
                            I[y0 + i * dy + 1, x0 + j * dx + 1] = ret.rec_matrix[j, i];
                    }
            }
            #endregion

            return ret;
        }

        public static string NameOfBlockPredMode(BlockPredMode t)
        {
            string ret = null;
            switch (t)
            {
                case BlockPredMode.Backward:
                    ret = "Backward";
                    break;
                case BlockPredMode.Direct:
                    ret = "Direct";
                    break;
                case BlockPredMode.Forward:
                    ret = "Forward";
                    break;
                case BlockPredMode.Skip:
                    ret = "Skip";
                    break;
                case BlockPredMode.Symmetric:
                    ret = "Symmetric";
                    break;
            }
            return ret;
        }

        void GetBlockDistances()
        {
            if (ref_block_bw != null)
                block_distance = block_distance_bw = (-distance_index + ref_block_bw.distance_index + 512) % 512;
            if (ref_block_fw != null)
                block_distance = block_distance_fw = (distance_index - ref_block_fw.distance_index + 512) % 512;
            if (ref_block != null)
            {
                if (inter_pred_mode != BlockPredMode.Backward)
                    block_distance = (distance_index - ref_block.distance_index + 512) % 512;
                else
                    block_distance = (-distance_index + ref_block.distance_index + 512) % 512;
            }
        }

        void GetReferenceBlocksAndMotionVectors()
        {
            int x = (int)(Parameters.MbIndex % Context.currentSequence.mb_width);
            int y = (int)(Parameters.MbIndex / Context.currentSequence.mb_width);
            int half_height = (int)Context.currentSequence.mb_height / 2;

            #region Forward & backward
            if (inter_pred_mode == BlockPredMode.Forward)
            {
                MacroBlock ref_mb = null;
                if (Context.currentPicture.picture_structure == 1)
                {
                    if (Context.currentPicture.picture_type != 2 && ref_index == 0)
                        ref_mb = Parameters.BackwardRef.BlockMatrix[y, x];
                    else
                        ref_mb = Parameters.ForwardRef.BlockMatrix[y, x];
                }
                else
                {
                    Picture fw_pic = null;
                    int idx = ref_index;
                    if (Context.currentPicture.picture_type == 2)
                        fw_pic = Parameters.ForwardRef;
                    else
                        fw_pic = Parameters.BackwardRef;
                    if (y >= half_height)
                    {
                        if (Context.currentPicture.picture_type == 2)
                            idx++;
                        if (idx == 0)
                            ref_mb = Context.currentPicture.BlockMatrix[y - half_height, x];
                        else if (idx == 1)
                            ref_mb = fw_pic.BlockMatrix[y, x];
                        else if (idx == 2)
                            ref_mb = fw_pic.BlockMatrix[y - half_height, x];
                        else
                            ref_mb = Parameters.ForwardRef.BlockMatrix[y, x];
                    }
                    else
                    {
                        if (idx == 0)
                            ref_mb = fw_pic.BlockMatrix[y + half_height, x];
                        else if (idx == 1)
                            ref_mb = fw_pic.BlockMatrix[y, x];
                        else if (idx == 2)
                            ref_mb = Parameters.ForwardRef.BlockMatrix[y + half_height, x];
                        else
                            ref_mb = Parameters.ForwardRef.BlockMatrix[y, x];
                    }
                }
                ref_block_fw = ref_block = ref_mb.blocks[id];
                mvFw = mvE = Parameters.MVEPred;
                return;
            }

            if (inter_pred_mode == BlockPredMode.Backward)
            {
                if (Context.currentPicture.picture_structure == 1)
                    ref_block = Parameters.BackwardRef.BlockMatrix[y, x].blocks[id];
                else
                {
                    if (y >= half_height)
                    {
                        if (ref_index == 0)
                            ref_block = Parameters.BackwardRef.BlockMatrix[y - half_height, x].blocks[id];
                        else
                            ref_block = Parameters.BackwardRef.BlockMatrix[y, x].blocks[id];
                    }
                    else
                    {
                        if (ref_index == 0)
                            ref_block = Parameters.BackwardRef.BlockMatrix[y, x].blocks[id];
                        else
                            ref_block = Parameters.BackwardRef.BlockMatrix[y + half_height, x].blocks[id];
                    }
                }
                ref_block_bw = ref_block;
                mvBw = mvE = Parameters.MVEPred;
                return;
            }
            #endregion
            #region P_Skip
            if (inter_pred_mode == BlockPredMode.Skip && Context.currentPicture.picture_type != 2)
            {
                if (Context.currentPicture.picture_structure == 1)
                    ref_block = Parameters.BackwardRef.BlockMatrix[y, x].blocks[id];
                else
                {
                    if (y >= half_height)
                        ref_block = Context.currentPicture.BlockMatrix[y - half_height, x].blocks[id];
                    else
                        ref_block = Parameters.BackwardRef.BlockMatrix[y + half_height, x].blocks[id];
                }
                if (x == 0 || y <= Context.currentSlice.slice_vertical_position)
                    mvE = MotionVector.zero();
                else if (
                    Context.currentPicture.BlockMatrix[y, x - 1].blocks[1].mvE == 0
                    && Context.currentPicture.BlockMatrix[y, x - 1].blocks[1].ref_index == 0
                    || Context.currentPicture.BlockMatrix[y - 1, x].blocks[2].mvE == 0
                    && Context.currentPicture.BlockMatrix[y - 1, x].blocks[2].ref_index == 0)
                    mvE = MotionVector.zero();
                else
                    mvE = Parameters.MVEPred;
                ref_block_fw = ref_block;
                return;
            }
            #endregion
            #region B_Skip | Direct
            if(inter_pred_mode == BlockPredMode.Skip && Context.currentPicture.picture_type == 2
                || inter_pred_mode == BlockPredMode.Direct)
            {
                if (Parameters.BackwardRef.BlockMatrix[y, x].blocks[id].intra)
                {
                    ref_index_fw = ref_index_bw = 0;
                    if (Context.currentPicture.picture_structure == 1)
                    {
                        ref_block_fw = Parameters.ForwardRef.BlockMatrix[y, x].blocks[id];
                        ref_block_bw = Parameters.BackwardRef.BlockMatrix[y, x].blocks[id];
                    }
                    else
                    {
                        if (y >= half_height)
                        {
                            ref_block_fw = Parameters.ForwardRef.BlockMatrix[y, x].blocks[id];
                            ref_block_bw = Parameters.BackwardRef.BlockMatrix[y - half_height, x].blocks[id];
                        }
                        else
                        {
                            ref_block_fw = Parameters.ForwardRef.BlockMatrix[y + half_height, x].blocks[id];
                            ref_block_bw = Parameters.BackwardRef.BlockMatrix[y, x].blocks[id];
                        }
                    }
                    ref_block = ref_block_fw;
                    mvE = mvFw = Parameters.MVEPredFw;
                    mvBw = Parameters.MVEPredBw;
                    return;
                }
                if (Context.currentPicture.picture_structure == 1)
                {
                    if (Parameters.BackwardRef.picture_structure == 1)
                    {
                        ref_block_fw = Parameters.ForwardRef.BlockMatrix[y, x].blocks[id];
                        ref_block_bw = Parameters.BackwardRef.BlockMatrix[y, x].blocks[id];
                    }
                    else
                    {
                        // This is weird
                        // TODO: Implement this
                        throw new Exception("Invalid AVS format!");
                    }
                }
                else
                {
                    if (Parameters.BackwardRef.picture_structure == 0)
                    {
                        ref_block_bw = Parameters.BackwardRef.BlockMatrix[y, x].blocks[id]; // Correct?
                        Block fw0, fw1;
                        if (y >= half_height)
                        {
                            fw0 = Parameters.ForwardRef.BlockMatrix[y, x].blocks[id];
                            fw1 = Parameters.ForwardRef.BlockMatrix[y - half_height, x].blocks[id];
                        }
                        else
                        {
                            fw0 = Parameters.ForwardRef.BlockMatrix[y + half_height, x].blocks[id];
                            fw1 = Parameters.ForwardRef.BlockMatrix[y, x].blocks[id];
                        }
                        if (ref_block_bw.ref_block.distance_index == fw0.distance_index)
                            ref_block_fw = fw0;
                        else
                            ref_block_fw = fw1;
                    }
                    else
                    {
                        // This is weird
                        // TODO: Implement this
                        throw new Exception("Invalid AVS format!");
                    }
                }
                ref_block = ref_block_fw;
                GetBlockDistances();
                int vx, vy;
                if (ref_block_bw.mvE.x < 0)
                    vx = -(((16384 / ref_block_bw.block_distance) * (1 - ref_block_bw.mvE.x * block_distance_fw) - 1) >> 14);
                else
                    vx = ((16384 / ref_block_bw.block_distance) * (1 + ref_block_bw.mvE.x * block_distance_fw) - 1) >> 14;
                if (ref_block_bw.mvE.y < 0)
                    vy = -(((16384 / ref_block_bw.block_distance) * (1 - ref_block_bw.mvE.y * block_distance_fw) - 1) >> 14);
                else
                    vy = ((16384 / ref_block_bw.block_distance) * (1 + ref_block_bw.mvE.y * block_distance_fw) - 1) >> 14;
                mvE = mvFw = new MotionVector(vx, vy);
                if (ref_block_bw.mvE.x < 0)
                    vx = ((16384 / ref_block_bw.block_distance) * (1 - ref_block_bw.mvE.x * block_distance_bw) - 1) >> 14;
                else
                    vx = -(((16384 / ref_block_bw.block_distance) * (1 + ref_block_bw.mvE.x * block_distance_bw) - 1) >> 14);
                if (ref_block_bw.mvE.y < 0)
                    vy = ((16384 / ref_block_bw.block_distance) * (1 - ref_block_bw.mvE.y * block_distance_bw) - 1) >> 14;
                else
                    vy = -(((16384 / ref_block_bw.block_distance) * (1 + ref_block_bw.mvE.y * block_distance_bw) - 1) >> 14);
                mvBw = new MotionVector(vx, vy);
                return;
            }
            #endregion
            #region Symmetric
            if (inter_pred_mode == BlockPredMode.Symmetric)
            {
                ref_index_bw = ref_index_fw = ref_index;
                if (Context.currentPicture.picture_structure == 1)
                {
                    ref_block_fw = Parameters.ForwardRef.BlockMatrix[y, x].blocks[id];
                    ref_block_bw = Parameters.BackwardRef.BlockMatrix[y, x].blocks[id];
                }
                else
                {
                    ref_index_bw = 1 - ref_index;
                    if (y >= half_height)
                    {
                        if (ref_index == 0)
                        {
                            ref_block_fw = Parameters.ForwardRef.BlockMatrix[y - half_height, x].blocks[id];
                            ref_block_bw = Parameters.BackwardRef.BlockMatrix[y, x].blocks[id];
                        }
                        else
                        {
                            ref_block_fw = Parameters.ForwardRef.BlockMatrix[y, x].blocks[id];
                            ref_block_bw = Parameters.BackwardRef.BlockMatrix[y - half_height, x].blocks[id];
                        }
                    }
                    else
                    {
                        if (ref_index == 0)
                        {
                            ref_block_fw = Parameters.ForwardRef.BlockMatrix[y, x].blocks[id];
                            ref_block_bw = Parameters.BackwardRef.BlockMatrix[y + half_height, x].blocks[id];
                        }
                        else
                        {
                            ref_block_fw = Parameters.ForwardRef.BlockMatrix[y + half_height, x].blocks[id];
                            ref_block_bw = Parameters.BackwardRef.BlockMatrix[y, x].blocks[id];
                        }
                    }
                }
                mvE = mvFw = Parameters.MVEPred;
                ref_block = ref_block_fw;
                GetBlockDistances();
                int vx = -((mvFw.x * block_distance_bw * (512 / block_distance_fw) + 256) >> 9);
                int vy = -((mvFw.y * block_distance_bw * (512 / block_distance_fw) + 256) >> 9);
                mvBw = new MotionVector(vx, vy);
            }
            #endregion
        }

        void PredictMotionVectors()
        {
            int x = (int)(Parameters.MbIndex % Context.currentSequence.mb_width);
            int y = (int)(Parameters.MbIndex / Context.currentSequence.mb_width);
            int slicey = (int)(Context.currentSlice.slice_vertical_position);
            Block A = null, B = null, C = null, D = null;
            string name = MacroBlock.NameOfType(Context.currentMB.type);
            #region Block 8*8
            if (name.Contains("8x8"))
            {
                if (id == 0)
                {
                    if (x > 0) A = Context.currentPicture.BlockMatrix[y, x - 1].blocks[1];
                    if (y > slicey) B = Context.currentPicture.BlockMatrix[y - 1, x].blocks[2];
                    if (x > 0 && y > slicey) D = Context.currentPicture.BlockMatrix[y - 1, x - 1].blocks[3];
                    if (y > slicey) C = Context.currentPicture.BlockMatrix[y - 1, x].blocks[3];
                }
                else if (id == 1)
                {
                    A = Context.currentMB.blocks[0];
                    if (y > slicey) B = Context.currentPicture.BlockMatrix[y - 1, x].blocks[3];
                    if (y > slicey) D = Context.currentPicture.BlockMatrix[y - 1, x].blocks[2];
                    if (y > slicey && x + 1 < Context.currentSequence.mb_width)
                        C = Context.currentPicture.BlockMatrix[y - 1, x + 1].blocks[2];
                }
                else if (id == 2)
                {
                    if (x > 0) A = Context.currentPicture.BlockMatrix[y, x - 1].blocks[3];
                    B = Context.currentMB.blocks[0];
                    if (x > 0) D = Context.currentPicture.BlockMatrix[y, x - 1].blocks[1];
                    C = Context.currentMB.blocks[1];
                }
                else
                {
                    A = Context.currentMB.blocks[2];
                    B = Context.currentMB.blocks[1];
                    D = Context.currentMB.blocks[0];
                }
            }
            #endregion
            #region Block 16*8
            else if (name.Contains("16x8"))
            {
                if (id < 2)
                {
                    if (x > 0) A = Context.currentPicture.BlockMatrix[y, x - 1].blocks[1];
                    if (y > slicey) B = Context.currentPicture.BlockMatrix[y - 1, x].blocks[2];
                    if (x > 0 && y > slicey) D = Context.currentPicture.BlockMatrix[y - 1, x - 1].blocks[3];
                    if (y > slicey && x + 1 < Context.currentSequence.mb_width)
                        C = Context.currentPicture.BlockMatrix[y - 1, x + 1].blocks[2];
                }
                else
                {
                    if (x > 0) A = Context.currentPicture.BlockMatrix[y, x - 1].blocks[3];
                    B = Context.currentMB.blocks[0];
                    if (x > 0) D = Context.currentPicture.BlockMatrix[y, x - 1].blocks[1];
                }
            }
            #endregion
            #region Block 8*16
            else if (name.Contains("8x16"))
            {
                if (id % 2 == 0)
                {
                    if (x > 0) A = Context.currentPicture.BlockMatrix[y, x - 1].blocks[1];
                    if (y > slicey) B = Context.currentPicture.BlockMatrix[y - 1, x].blocks[2];
                    if (x > 0 && y > slicey) D = Context.currentPicture.BlockMatrix[y - 1, x - 1].blocks[3];
                    if (y > slicey) C = Context.currentPicture.BlockMatrix[y - 1, x].blocks[3];
                }
                else
                {
                    A = Context.currentMB.blocks[0];
                    if (y > slicey) B = Context.currentPicture.BlockMatrix[y - 1, x].blocks[3];
                    if (y > slicey) D = Context.currentPicture.BlockMatrix[y - 1, x].blocks[2];
                    if (y > slicey && x + 1 < Context.currentSequence.mb_width)
                        C = Context.currentPicture.BlockMatrix[y - 1, x + 1].blocks[2];
                }
            }
            #endregion
            #region Block 16*16
            else
            {
                if (x > 0) A = Context.currentPicture.BlockMatrix[y, x - 1].blocks[1];
                if (y > slicey) B = Context.currentPicture.BlockMatrix[y - 1, x].blocks[2];
                if (x > 0 && y > slicey) D = Context.currentPicture.BlockMatrix[y - 1, x - 1].blocks[3];
                if (y > slicey && x + 1 < Context.currentSequence.mb_width)
                    C = Context.currentPicture.BlockMatrix[y - 1, x + 1].blocks[2];
            }
            #endregion

            // Watch the Block as 16*16 according to the standard
            if (inter_pred_mode == BlockPredMode.Skip && Context.currentPicture.picture_type == 2
                || inter_pred_mode == BlockPredMode.Direct)
            {
                A = B = C = D = null;
                if (x > 0) A = Context.currentPicture.BlockMatrix[y, x - 1].blocks[1];
                if (y > slicey) B = Context.currentPicture.BlockMatrix[y - 1, x].blocks[2];
                if (x > 0 && y > slicey) D = Context.currentPicture.BlockMatrix[y - 1, x - 1].blocks[3];
                if (y > slicey && x + 1 < Context.currentSequence.mb_width)
                    C = Context.currentPicture.BlockMatrix[y - 1, x + 1].blocks[2];
            }

            #region ~Backward inter pred
            if (inter_pred_mode != BlockPredMode.Backward)
            {
                BlockPredMode bak_md = inter_pred_mode;
                inter_pred_mode = BlockPredMode.Forward;
                if (!HaveSameDirection(A, this))
                    A = null;
                if (!HaveSameDirection(B, this))
                    B = null;
                if (!HaveSameDirection(D, this))
                    D = null;
                if (C == null) C = D;
                if (!HaveSameDirection(C, this))
                    C = null;
                inter_pred_mode = bak_md;

                bool flag = false;
                if (A != null && B == null && C == null)
                {
                    Parameters.MVEPred = A.mvE;
                    flag = true;
                }
                else if (A == null && B != null && C == null)
                {
                    Parameters.MVEPred = B.mvE;
                    flag = true;
                }
                else if (A == null && B == null && C != null)
                {
                    Parameters.MVEPred = C.mvE;
                    flag = true;
                }
                if (!flag)
                {
                    if (name.Contains("8x16"))
                    {
                        if (id % 2 == 0 && A != null && A.ref_index == ref_index)
                        {
                            Parameters.MVEPred = A.mvE;
                            flag = true;
                        }
                        if (id % 2 == 1 && C != null && C.ref_index == ref_index)
                        {
                            Parameters.MVEPred = C.mvE;
                            flag = true;
                        }
                    }
                    if (name.Contains("16x8"))
                    {
                        if (id < 2 && B != null && B.ref_index == ref_index)
                        {
                            Parameters.MVEPred = B.mvE;
                            flag = true;
                        }
                        if (id >= 2 && A != null && A.ref_index == ref_index)
                        {
                            Parameters.MVEPred = A.mvE;
                            flag = true;
                        }
                    }
                }
                if (!flag)
                {
                    MotionVector mva = MotionVector.zero(),
                        mvb = MotionVector.zero(),
                        mvc = MotionVector.zero();
                    int bda = 1, bdb = 1, bdc = 1;
                    if (A != null)
                    {
                        bda = A.block_distance;
                        mva = A.mvE;
                    }
                    if (B != null)
                    {
                        bdb = B.block_distance;
                        mvb = B.mvE;
                    }
                    if (C != null)
                    {
                        bdc = C.block_distance;
                        mvc = C.mvE;
                    }
                    int vax = Math.Sign(mva.x) * ((Math.Abs(mva.x) * block_distance * (512 / bda) + 256) >> 9);
                    int vay = Math.Sign(mva.y) * ((Math.Abs(mva.y) * block_distance * (512 / bda) + 256) >> 9);
                    int vbx = Math.Sign(mvb.x) * ((Math.Abs(mvb.x) * block_distance * (512 / bdb) + 256) >> 9);
                    int vby = Math.Sign(mvb.y) * ((Math.Abs(mvb.y) * block_distance * (512 / bdb) + 256) >> 9);
                    int vcx = Math.Sign(mvc.x) * ((Math.Abs(mvc.x) * block_distance * (512 / bdc) + 256) >> 9);
                    int vcy = Math.Sign(mvc.y) * ((Math.Abs(mvc.y) * block_distance * (512 / bdc) + 256) >> 9);
                    int vab = Math.Abs(vax - vbx) + Math.Abs(vay - vby);
                    int vbc = Math.Abs(vbx - vcx) + Math.Abs(vby - vcy);
                    int vca = Math.Abs(vcx - vax) + Math.Abs(vcy - vay);
                    if (vab >= vca && vab <= vbc || vab <= vca && vab >= vbc)
                        Parameters.MVEPred = new MotionVector(vcx, vcy);
                    else if (vbc >= vca && vbc <= vab || vbc <= vca && vbc >= vab)
                        Parameters.MVEPred = new MotionVector(vax, vay);
                    else
                        Parameters.MVEPred = new MotionVector(vbx, vby);
                }
            }
            #endregion
            #region Backward inter pred
            else
            {
                if (!HaveSameDirection(A, this))
                    A = null;
                if (!HaveSameDirection(B, this))
                    B = null;
                if (!HaveSameDirection(D, this))
                    D = null;
                if (C == null) C = D;
                if (!HaveSameDirection(C, this))
                    C = null; 
                
                bool flag = false;
                if (A != null && B == null && C == null)
                {
                    Parameters.MVEPred = A.mvBw;
                    flag = true;
                }
                else if (A == null && B != null && C == null)
                {
                    Parameters.MVEPred = B.mvBw;
                    flag = true;
                }
                else if (A == null && B == null && C != null)
                {
                    Parameters.MVEPred = C.mvBw;
                    flag = true;
                }
                if (!flag)
                {
                    if (name.Contains("8x16"))
                    {
                        if (id % 2 == 0 && A != null && A.ref_index == ref_index)
                        {
                            Parameters.MVEPred = A.mvBw;
                            flag = true;
                        }
                        if (id % 2 == 1 && C != null && C.ref_index == ref_index)
                        {
                            Parameters.MVEPred = C.mvBw;
                            flag = true;
                        }
                    }
                    if (name.Contains("16x8"))
                    {
                        if (id < 2 && B != null && B.ref_index == ref_index)
                        {
                            Parameters.MVEPred = B.mvBw;
                            flag = true;
                        }
                        if (id >= 2 && A != null && A.ref_index == ref_index)
                        {
                            Parameters.MVEPred = A.mvBw;
                            flag = true;
                        }
                    }
                }
                if (!flag)
                {
                    MotionVector mva = MotionVector.zero(),
                        mvb = MotionVector.zero(),
                        mvc = MotionVector.zero();
                    int bda = 1, bdb = 1, bdc = 1;
                    if (A != null)
                    {
                        bda = A.block_distance_bw;
                        if (bda == 0) bda = A.block_distance;
                        mva = A.mvBw;
                    }
                    if (B != null)
                    {
                        bdb = B.block_distance_bw;
                        if (bdb == 0) bdb = B.block_distance;
                        mvb = B.mvBw;
                    }
                    if (C != null)
                    {
                        bdc = C.block_distance_bw;
                        if (bdc == 0) bdc = C.block_distance;
                        mvc = C.mvBw;
                    }
                    int vax = Math.Sign(mva.x) * ((Math.Abs(mva.x) * block_distance * (512 / bda) + 256) >> 9);
                    int vay = Math.Sign(mva.y) * ((Math.Abs(mva.y) * block_distance * (512 / bda) + 256) >> 9);
                    int vbx = Math.Sign(mvb.x) * ((Math.Abs(mvb.x) * block_distance * (512 / bdb) + 256) >> 9);
                    int vby = Math.Sign(mvb.y) * ((Math.Abs(mvb.y) * block_distance * (512 / bdb) + 256) >> 9);
                    int vcx = Math.Sign(mvc.x) * ((Math.Abs(mvc.x) * block_distance * (512 / bdc) + 256) >> 9);
                    int vcy = Math.Sign(mvc.y) * ((Math.Abs(mvc.y) * block_distance * (512 / bdc) + 256) >> 9);
                    int vab = Math.Abs(vax - vbx) + Math.Abs(vay - vby);
                    int vbc = Math.Abs(vbx - vcx) + Math.Abs(vby - vcy);
                    int vca = Math.Abs(vcx - vax) + Math.Abs(vcy - vay);
                    if (vab >= vca && vab <= vbc || vab <= vca && vab >= vbc)
                        Parameters.MVEPred = new MotionVector(vcx, vcy);
                    else if (vbc >= vca && vbc <= vab || vbc <= vca && vbc >= vab)
                        Parameters.MVEPred = new MotionVector(vax, vay);
                    else
                        Parameters.MVEPred = new MotionVector(vbx, vby);
                }
            }
            #endregion

            Parameters.MVEPred.x += Parameters.MVDiffX;
            Parameters.MVEPred.y += Parameters.MVDiffY;

            #region B_Skip | Direct
            if (inter_pred_mode == BlockPredMode.Skip && Context.currentPicture.picture_type == 2
                || inter_pred_mode == BlockPredMode.Direct)
            {
                Parameters.MVEPredFw = Parameters.MVEPred;

                // Watch the Block as 16*16 according to the standard
                A = B = C = D = null;
                if (x > 0) A = Context.currentPicture.BlockMatrix[y, x - 1].blocks[1];
                if (y > slicey) B = Context.currentPicture.BlockMatrix[y - 1, x].blocks[2];
                if (x > 0 && y > slicey) D = Context.currentPicture.BlockMatrix[y - 1, x - 1].blocks[3];
                if (y > slicey && x + 1 < Context.currentSequence.mb_width)
                    C = Context.currentPicture.BlockMatrix[y - 1, x + 1].blocks[2];

                BlockPredMode bak = inter_pred_mode;
                inter_pred_mode = BlockPredMode.Backward;
                if (!HaveSameDirection(this, A))
                    A = null;
                if (!HaveSameDirection(this, B))
                    B = null;
                if (!HaveSameDirection(this, D))
                    D = null;
                if (C == null) C = D;
                if (!HaveSameDirection(this, C))
                    C = null;
                inter_pred_mode = bak;

                bool flag = false;
                if (A != null && B == null && C == null)
                {
                    Parameters.MVEPredBw = A.mvBw;
                    flag = true;
                }
                else if (A == null && B != null && C == null)
                {
                    Parameters.MVEPredBw = B.mvBw;
                    flag = true;
                }
                else if (A == null && B == null && C != null)
                {
                    Parameters.MVEPredBw = C.mvBw;
                    flag = true;
                }
                if (!flag)
                {
                    MotionVector mva = MotionVector.zero(),
                        mvb = MotionVector.zero(),
                        mvc = MotionVector.zero();
                    int bda = 1, bdb = 1, bdc = 1;
                    if (A != null)
                    {
                        bda = A.block_distance_bw;
                        if (bda == 0) bda = A.block_distance;
                        mva = A.mvBw;
                    }
                    if (B != null)
                    {
                        bdb = B.block_distance_bw;
                        if (bdb == 0) bdb = B.block_distance;
                        mvb = B.mvBw;
                    }
                    if (C != null)
                    {
                        bdc = C.block_distance_bw;
                        if (bdc == 0) bdc = C.block_distance;
                        mvc = C.mvBw;
                    }
                    int vax = Math.Sign(mva.x) * ((Math.Abs(mva.x) * block_distance_bw * (512 / bda) + 256) >> 9);
                    int vay = Math.Sign(mva.y) * ((Math.Abs(mva.y) * block_distance_bw * (512 / bda) + 256) >> 9);
                    int vbx = Math.Sign(mvb.x) * ((Math.Abs(mvb.x) * block_distance_bw * (512 / bdb) + 256) >> 9);
                    int vby = Math.Sign(mvb.y) * ((Math.Abs(mvb.y) * block_distance_bw * (512 / bdb) + 256) >> 9);
                    int vcx = Math.Sign(mvc.x) * ((Math.Abs(mvc.x) * block_distance_bw * (512 / bdc) + 256) >> 9);
                    int vcy = Math.Sign(mvc.y) * ((Math.Abs(mvc.y) * block_distance_bw * (512 / bdc) + 256) >> 9);
                    int vab = Math.Abs(vax - vbx) + Math.Abs(vay - vby);
                    int vbc = Math.Abs(vbx - vcx) + Math.Abs(vby - vcy);
                    int vca = Math.Abs(vcx - vax) + Math.Abs(vcy - vay);
                    if (vab >= vca && vab <= vbc || vab >= vca && vab <= vbc)
                        Parameters.MVEPredBw = new MotionVector(vcx, vcy);
                    else if (vbc >= vca && vbc <= vab || vbc <= vca && vbc >= vab)
                        Parameters.MVEPredBw = new MotionVector(vax, vay);
                    else
                        Parameters.MVEPredBw = new MotionVector(vbx, vby);
                }
            }
            #endregion
        }

        static bool HaveSameDirection(Block a, Block b)
        {
            if (a == null || b == null || a.intra || b.intra)
                return false;
            if (a.inter_pred_mode == BlockPredMode.Backward
                && (b.inter_pred_mode == BlockPredMode.Forward
                || b.inter_pred_mode == BlockPredMode.Skip
                && b.picture.picture_type != 2))
                return false;
            if (b.inter_pred_mode == BlockPredMode.Backward
                && (a.inter_pred_mode == BlockPredMode.Forward
                || a.inter_pred_mode == BlockPredMode.Skip
                && a.picture.picture_type != 2))
                return false;
            return true;
        }

        int InterPredictLuma(int px, int py, int off_x, int off_y, MotionVector mv, int[,] img)
        {
            int ret = 0;
            int x = px + off_x + (mv.x / 4), y = py + off_y + (mv.y / 4);
            int dx = (mv.x & 3), dy = (mv.y & 3);
            int up = 0, down = img.GetLength(0) - 1, left = 0, right = img.GetLength(1) - 1;
            if (Context.currentPicture.picture_structure == 0)
            {
                if (py < img.GetLength(0) / 2)
                    down = img.GetLength(0) / 2 - 1;
                else
                    up = img.GetLength(0) / 2;
            }
            if (x < left) x = left;
            if (x > right) x = right;
            if (y < up) y = up;
            if (y > down) y = down;

            if (dx == 0)
            {
                if (dy == 0)
                    ret = img[y, x];
                else if (dy == 1)
                {
                    int[] tmp = new int[5];
                    for (int i = -2; i <= 2; ++i)
                    {
                        if (y + i < up)
                            tmp[i + 2] = img[up, x];
                        else if (y + i > down)
                            tmp[i + 2] = img[down, x];
                        else
                            tmp[i + 2] = img[y + i, x];
                    }
                    int f = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    int h = (-tmp[1] + 5 * tmp[2] + 5 * tmp[3] - tmp[4]);
                    tmp[2] <<= 3;
                    tmp[3] <<= 3;
                    ret = (f + 7 * tmp[2] + 7 * h + tmp[3] + 64) >> 7;
                }
                else if (dy == 2)
                {
                    int[] tmp = new int[4];
                    for (int i = -1; i <= 2; ++i)
                    {
                        if (y + i < up)
                            tmp[i + 1] = img[up, x];
                        else if (y + i > down)
                            tmp[i + 1] = img[down, x];
                        else
                            tmp[i + 1] = img[y + i, x];
                    }
                    ret = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3] + 4) >> 3;
                }
                else
                {
                    int[] tmp = new int[5];
                    for (int i = 3; i >= -1; --i)
                    {
                        if (y + i < up)
                            tmp[3 - i] = img[up, x];
                        else if (y + i > down)
                            tmp[3 - i] = img[down, x];
                        else
                            tmp[3 - i] = img[y + i, x];
                    }
                    int f = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    int h = (-tmp[1] + 5 * tmp[2] + 5 * tmp[3] - tmp[4]);
                    tmp[2] <<= 3;
                    tmp[3] <<= 3;
                    ret = (f + 7 * tmp[2] + 7 * h + tmp[3] + 64) >> 7;
                }
            }
            else if (dx == 1)
            {
                if (dy == 0)
                {
                    int[] tmp = new int[5];
                    for (int i = -2; i <= 2; ++i)
                    {
                        if (x + i < left)
                            tmp[i + 2] = img[y, left];
                        else if (x + i > right)
                            tmp[i + 2] = img[y, right];
                        else
                            tmp[i + 2] = img[y, x + i];
                    }
                    int f = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    int h = (-tmp[1] + 5 * tmp[2] + 5 * tmp[3] - tmp[4]);
                    tmp[2] <<= 3;
                    tmp[3] <<= 3;
                    ret = (f + 7 * tmp[2] + 7 * h + tmp[3] + 64) >> 7;
                }
                else if (dy == 1)
                {
                    int[] tmp = new int[5];
                    for (int i = -2; i <= 2; ++i)
                    {
                        if (x + i < left || x + i > right || y + i < up || y + i > down)
                            tmp[i + 2] = img[y, x];
                        else
                            tmp[i + 2] = img[y + i, x + i];
                    }
                    int f = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    int h = (-tmp[1] + 5 * tmp[2] + 5 * tmp[3] - tmp[4]);
                    tmp[2] <<= 3;
                    tmp[3] <<= 3;
                    ret = (f + 7 * tmp[2] + 7 * h + tmp[3] + 64) >> 7;
                }
                else if (dy == 2)
                {
                    int g, h, j, m;
                    int[] tmp = new int[4];
                    for (int i = -1; i <= 2; ++i)
                    {
                        if (x - i < left || x - i > right || y + i < up || y + i > down)
                            tmp[i + 1] = img[y, x];
                        else
                            tmp[i + 1] = img[y + i, x - i];
                    }
                    g = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    for (int i = -1; i <= 2; ++i)
                    {
                        if (y + i < up)
                            tmp[i + 1] = img[up, x];
                        else if (y + i > down)
                            tmp[i + 1] = img[down, x];
                        else
                            tmp[i + 1] = img[y + i, x];
                    }
                    h = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    for (int i = -1; i <= 2; ++i)
                    {
                        if (x + i < left || x + i > right || y + i < up || y + i > down)
                            tmp[i + 1] = img[y, x];
                        else
                            tmp[i + 1] = img[y + i, x + i];
                    }
                    j = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    if (x + 1 > right)
                        m = j;
                    else
                    {
                        for (int i = -1; i <= 2; ++i)
                        {
                            if (y + i < up)
                                tmp[i + 1] = img[up, x + 1];
                            else if (y + i > down)
                                tmp[i + 1] = img[down, x + 1];
                            else
                                tmp[i + 1] = img[y + i, x + 1];
                        }
                        m = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    }
                    ret = (g + 7 * h + 7 * j + m + 64) >> 7;
                }
                else
                {
                    int[] tmp = new int[5];
                    for (int i = -2; i <= 2; ++i)
                    {
                        if (x - i < left || x - i > right || y + i + 1 < up || y + i + 1 > down)
                            tmp[i + 2] = img[y, x];
                        else
                            tmp[i + 2] = img[y + i + 1, x - i];
                    }
                    int f = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    int h = (-tmp[1] + 5 * tmp[2] + 5 * tmp[3] - tmp[4]);
                    tmp[1] <<= 3;
                    tmp[2] <<= 3;
                    ret = (h + 7 * tmp[2] + 7 * f + tmp[1] + 64) >> 7;
                }
            }
            else if (dx == 2)
            {
                if (dy == 0)
                {
                    int[] tmp = new int[4];
                    for (int i = -1; i <= 2; ++i)
                    {
                        if (x + i < left)
                            tmp[i + 1] = img[y, left];
                        else if (x + i > right)
                            tmp[i + 1] = img[y, right];
                        else
                            tmp[i + 1] = img[y, x + i];
                    }
                    ret = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3] + 4) >> 3;
                }
                else if (dy == 1)
                {
                    int h, b, j, s;
                    int[] tmp = new int[4];
                    for(int i=-1; i<=2; ++i)
                    {
                        if (x + i < left || x + i > right || y - i < up || y - i > down)
                            tmp[i + 1] = img[y, x];
                        else
                            tmp[i + 1] = img[y - i, x + i];
                    }
                    h = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    for (int i = -1; i <= 2; ++i)
                    {
                        if (x + i < left)
                            tmp[i + 1] = img[y, left];
                        else if (x + i > right)
                            tmp[i + 1] = img[y, right];
                        else
                            tmp[i + 1] = img[y, x + i];
                    }
                    b = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    for (int i = -1; i <= 2; ++i)
                    {
                        if (x + i < left || x + i > right || y + i < up || y + i > down)
                            tmp[i + 1] = img[y, x];
                        else
                            tmp[i + 1] = img[y + i, x + i];
                    }
                    j = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    if (y + 1 > down)
                        s = j;
                    else
                    {
                        for (int i = -1; i <= 2; ++i)
                        {
                            if (x + i < left)
                                tmp[i + 1] = img[y + 1, left];
                            else if (x + i > right)
                                tmp[i + 1] = img[y + 1, right];
                            else
                                tmp[i + 1] = img[y + 1, x + i];
                        }
                        s = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    }
                    ret = (h + 7 * b + 7 * j + s + 64) >> 7;
                }
                else if (dy == 2)
                {
                    int[] tmp = new int[4];
                    for (int i = -1; i <= 2; ++i)
                    {
                        if (x + i < left || x + i > right || y + i < up || y + i > down)
                            tmp[i + 1] = img[y, x];
                        else
                            tmp[i + 1] = img[y + i, x + i];
                    }
                    ret = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3] + 4) >> 3;
                }
                else
                {
                    int h, b, j, s;
                    int[] tmp = new int[4];
                    for (int i = -1; i <= 2; ++i)
                    {
                        if (x + i < left || x + i > right || y + i + 1 < up || y + i + 1 > down)
                            tmp[i + 1] = img[y, x];
                        else
                            tmp[i + 1] = img[y + i + 1, x + i];
                    }
                    h = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    for (int i = -1; i <= 2; ++i)
                    {
                        if (x + i < left)
                            tmp[i + 1] = img[y, left];
                        else if (x + i > right)
                            tmp[i + 1] = img[y, right];
                        else
                            tmp[i + 1] = img[y, x + i];
                    }
                    b = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    for (int i = -1; i <= 2; ++i)
                    {
                        if (x + i < left || x + i > right || y + i < up || y + i > down)
                            tmp[i + 1] = img[y, x];
                        else
                            tmp[i + 1] = img[y + i, x + i];
                    }
                    j = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    if (y + 1 > down)
                        s = j;
                    else
                    {
                        for (int i = -1; i <= 2; ++i)
                        {
                            if (x + i < left)
                                tmp[i + 1] = img[y + 1, left];
                            else if (x + i > right)
                                tmp[i + 1] = img[y + 1, right];
                            else
                                tmp[i + 1] = img[y + 1, x + i];
                        }
                        s = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    }
                    ret = (h + 7 * s + 7 * j + b + 64) >> 7;
                }
            }
            else
            {
                if (dy == 0)
                {
                    int[] tmp = new int[5];
                    for (int i = 2; i >= -2; --i)
                    {
                        if (x + i < left)
                            tmp[2 - i] = img[y, left];
                        else if (x + i > right)
                            tmp[2 - i] = img[y, right];
                        else
                            tmp[2 - i] = img[y, x + i];
                    }
                    int f = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    int h = (-tmp[1] + 5 * tmp[2] + 5 * tmp[3] - tmp[4]);
                    tmp[2] <<= 3;
                    tmp[3] <<= 3;
                    ret = (f + 7 * tmp[2] + 7 * h + tmp[3] + 64) >> 7;
                }
                else if (dy == 1)
                {
                    int[] tmp = new int[5];
                    for (int i = -2; i <= 2; ++i)
                    {
                        if (x - i + 1 < left || x - i + 1 > right || y + i < up || y + i > down)
                            tmp[i + 2] = img[y, x];
                        else
                            tmp[i + 2] = img[y + i, x - i + 1];
                    }
                    int f = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    int h = (-tmp[1] + 5 * tmp[2] + 5 * tmp[3] - tmp[4]);
                    tmp[2] <<= 3;
                    tmp[3] <<= 3;
                    ret = (f + 7 * tmp[2] + 7 * h + tmp[3] + 64) >> 7;
                }
                else if (dy == 2)
                {
                    int g, h, j, m;
                    int[] tmp = new int[4];
                    for (int i = 0; i <= 3; ++i)
                    {
                        if (x + i < left || x + i > right || y + i - 1 < up || y + i - 1 > down)
                            tmp[i] = img[y, x];
                        else
                            tmp[i] = img[y + i - 1, x + i];
                    }
                    g = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    for (int i = -1; i <= 2; ++i)
                    {
                        if (y + i < up)
                            tmp[i + 1] = img[up, x];
                        else if (y + i > down)
                            tmp[i + 1] = img[down, x];
                        else
                            tmp[i + 1] = img[y + i, x];
                    }
                    h = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    for (int i = -1; i <= 2; ++i)
                    {
                        if (x + i < left || x + i > right || y + i < up || y + i > down)
                            tmp[i + 1] = img[y, x];
                        else
                            tmp[i + 1] = img[y + i, x + i];
                    }
                    j = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    if (x + 1 > right)
                        m = j;
                    else
                    {
                        for (int i = -1; i <= 2; ++i)
                        {
                            if (y + i < up)
                                tmp[i + 1] = img[up, x + 1];
                            else if (y + i > down)
                                tmp[i + 1] = img[down, x + 1];
                            else
                                tmp[i + 1] = img[y + i, x + 1];
                        }
                        m = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    }
                    ret = (h + 7 * j + 7 * m + g + 64) >> 7;
                }
                else
                {
                    int[] tmp = new int[5];
                    for (int i = 3; i >= -1; --i)
                    {
                        if (x + i < left || x + i > right || y + i < up || y + i > down)
                            tmp[3 - i] = img[y, x];
                        else
                            tmp[3 - i] = img[y + i, x + i];
                    }
                    int f = (-tmp[0] + 5 * tmp[1] + 5 * tmp[2] - tmp[3]);
                    int h = (-tmp[1] + 5 * tmp[2] + 5 * tmp[3] - tmp[4]);
                    tmp[2] <<= 3;
                    tmp[3] <<= 3;
                    ret = (f + 7 * tmp[2] + 7 * h + tmp[3] + 64) >> 7;
                }
            }

            return Decoder.Clip1(ret);
        }

        int InterPredictChroma(Block rb, int ox, int oy, MotionVector mv)
        {
            int ret = 0;
            int[,] img;
            if (id % 2 == 0)
                img = rb.picture.CbMatrix;
            else
                img = rb.picture.CrMatrix;
            ox *= 2;
            if (Context.currentSequence.chroma_format == 2)
                mv.y *= 2;
            else
                oy *= 2;
            int px = 16 * (rb.pos_x / 16), py = 16 * (rb.pos_y / 16);
            int x = px + ox + 2 * (mv.x / 8), y = py + oy + (mv.y / 8);
            int dx = (mv.x & 7), dy = (mv.y & 7);
            int up = 0, down = img.GetLength(0) - 1, left = 0, right = img.GetLength(1) - 1;
            if (Context.currentPicture.picture_structure == 0)
            {
                if (py < img.GetLength(0) / 2)
                    down = img.GetLength(0) / 2 - 1;
                else
                    up = img.GetLength(0) / 2;
            }
            if (x < left) x = left;
            if (x > right) x = right;
            if (y < up) y = up;
            if (y > down) y = down;

            int a, b, c, d;
            a = b = c = d = img[y, x];
            if (Context.currentSequence.chroma_format == 1)
            {
                if (x + 2 < right) b = img[y, x + 2];
                if (y + 2 < down) c = img[y + 2, x];
                if (x + 2 < right && y + 2 < down)
                    d = img[y + 2, x + 2];
            }
            else
            {
                if (x + 2 < right) b = img[y, x + 2];
                if (y + 1 < down) c = img[y + 1, x];
                if (x + 2 < right && y + 1 < down)
                    d = img[y + 1, x + 2];
            }

            ret = (8 - dx) * (8 - dy) * a + dx * (8 - dy) * b + (8 - dx) * dy * c + dx * dy * d;
            ret = (ret + 32) >> 6;

            return Decoder.Clip1(ret);
        }
    }
}
